#include "drv_gm6020.h"

#include "stdlib.h"

/* 电流指令缓冲区 */
uint8_t gmCmdBuf[4][8] = {0};

/**
 * @brief 初始化一个GM6020电机实例
 * @param motor: GM6020电机实例
 * @param id: 电机ID
 * @param mode: 电机控制模式
 * @param hfdcan: FDCAN控制器的句柄
 * @retval None
 */
void GM6020_Init(GM6020_t *motor, uint8_t id, GM6020Mode_t mode, FDCAN_HandleTypeDef *hfdcan)
{
    motor->motorID = id;
    motor->motorIDGroup = (motor->motorID <= 4) ? 0 : 1;
    motor->mode = mode;

    motor->hfdcan = hfdcan;
    motor->cmdStdID = (motor->mode == GM_VOLTAGE_CONTROL ? 0x1FF : 0x1FE) + motor->motorIDGroup * 0x100;
    motor->cmdBufRow = motor->mode + motor->motorIDGroup;
    motor->cmdBufColHigh = 2 * ((motor->motorID + motor->motorIDGroup) % 5) - 2;
    motor->cmdBufColLow = 2 * ((motor->motorID + motor->motorIDGroup) % 5) - 1;

    motor->initFlag = DISABLE;
    motor->initCnt = 0;
    motor->lastAngle = 0;
    motor->overflowCnt = 0;
}

/**
 * @brief 处理GM6020电机实例的控制指令
 * @param motor: GM6020电机实例
 * @param cmd: 控制指令值
 * @param mutex: 互斥锁
 * @retval None
 */
void GM6020_WriteCmdBuf(GM6020_t *motor, int16_t cmd, SemaphoreHandle_t mutex)
{
    if (xSemaphoreTake(mutex, 0) != pdTRUE)
    {
        return;
    }

    FDCAN_TxHeaderTypeDef txHeader = {0};
    txHeader.Identifier = motor->cmdStdID;
    txHeader.IdType = FDCAN_STANDARD_ID;
    txHeader.TxFrameType = FDCAN_DATA_FRAME;
    txHeader.DataLength = FDCAN_DLC_BYTES_8;
    txHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
    txHeader.BitRateSwitch = FDCAN_BRS_OFF;
    txHeader.FDFormat = FDCAN_CLASSIC_CAN;
    txHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
    txHeader.MessageMarker = 0x00;

    gmCmdBuf[motor->cmdBufRow][motor->cmdBufColHigh] = cmd >> 8;
    gmCmdBuf[motor->cmdBufRow][motor->cmdBufColLow] = cmd & 0xFF;
    if (HAL_FDCAN_GetTxFifoFreeLevel(motor->hfdcan) > 0)
    {
        HAL_FDCAN_AddMessageToTxFifoQ(motor->hfdcan, &txHeader, gmCmdBuf[motor->cmdBufRow] + 0);
    }

    xSemaphoreGive(mutex);
}

/**
 * @brief 此函数放置在ISR中, 更新GM6020电机实例的反馈信息
 * @param rxData: CAN接收的缓存数组
 * @param motor: GM6020电机实例
 * @param motorInfoBoxHandle: 存放电机反馈信息的邮箱句柄
 * @retval None
 */
void ISR_GM6020_UpdateInfo(uint8_t rxData[], GM6020_t *motor, QueueHandle_t motorInfoBoxHandle)
{
    if (motor->initFlag == DISABLE && ++(motor->initCnt) >= 32)
    {
        motor->lastAngle = (rxData[0] << 8) | rxData[1];
        motor->initFlag = ENABLE;
    }
    else if (motor->initFlag == ENABLE)
    {
        GM6020FeedbackMsg_t infoMsg = {0};

        infoMsg.angle = (rxData[0] << 8) | rxData[1];
        infoMsg.speed = (rxData[2] << 8) | rxData[3];
        infoMsg.torque = (rxData[4] << 8) | rxData[5];

        const int16_t temp = (int16_t)infoMsg.angle - (int16_t)motor->lastAngle;
        if (abs(temp) > 4000)
        {
            temp < 0 ? (motor->overflowCnt)++ : (motor->overflowCnt)--;
        }
        infoMsg.overflowCnt = motor->overflowCnt;

        motor->lastAngle = infoMsg.angle;

        xQueueOverwriteFromISR(motorInfoBoxHandle, &infoMsg, NULL);
    }
}

/**
 * @brief 获取GM6020的编码器计数
 * @param feedbackBoxHandle: 存放电机反馈信息的邮箱句柄
 * @return GM6020的编码器计数
 */
int32_t GM6020_GetCnt(QueueHandle_t feedbackBoxHandle)
{
    GM6020FeedbackMsg_t msg;
    xQueuePeek(feedbackBoxHandle, &msg, portMAX_DELAY);

    return (int32_t)msg.angle + (int32_t)msg.overflowCnt * 8192;
}

/**
 * @brief 获取GM6020的位置, 单位为度
 * @param feedbackBoxHandle: 存放电机反馈信息的邮箱句柄
 * @param baseCnt: 基准的编码器计数
 * @return GM6020的位置
 */
float GM6020_GetPos(QueueHandle_t feedbackBoxHandle, int32_t baseCnt)
{
    return (float)(GM6020_GetCnt(feedbackBoxHandle) - baseCnt) / 8192.0 * 360.0;
}

/**
 * @brief 获取GM6020的转速, 单位为rpm
 * @param feedbackBoxHandle: 存放电机反馈信息的邮箱句柄
 * @return GM6020的转速
 */
float GM6020_GetRpm(QueueHandle_t feedbackBoxHandle)
{
    GM6020FeedbackMsg_t msg;
    xQueuePeek(feedbackBoxHandle, &msg, portMAX_DELAY);

    return msg.speed;
}
